using System;
using System.Collections.Generic;

public class AStarPathfinding
{
    // 定义节点类
    public class Node
    {
        public int x;
        public int y;
        public bool isWalkable;
        public List<Node> neighbors;
        public Node parent;
        public int gCost; // 从起点到当前节点的实际代价
        public int hCost; // 当前节点到终点的启发式估计代价
        public int fCost { get { return gCost + hCost; } }

        public Node(int x, int y, bool isWalkable = true)
        {
            this.x = x;
            this.y = y;
            this.isWalkable = isWalkable;
            this.neighbors = new List<Node>();
        }
    }

    // A*算法核心实现
    public static List<Node> FindPath(Node startNode, Node targetNode)
    {
        List<Node> openSet = new List<Node>();
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            Node currentNode = openSet[0];
            for (int i = 1; i < openSet.Count; i++)
            {
                if (openSet[i].fCost < currentNode.fCost || 
                    (openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost))
                {
                    currentNode = openSet[i];
                }
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            // 找到目标节点
            if (currentNode == targetNode)
            {
                return RetracePath(startNode, targetNode);
            }

            // 遍历当前节点的所有邻居节点
            foreach (Node neighbor in currentNode.neighbors)
            {
                if (!neighbor.isWalkable || closedSet.Contains(neighbor))
                {
                    continue;
                }

                int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {
                    neighbor.gCost = newMovementCostToNeighbor;
                    neighbor.hCost = GetDistance(neighbor, targetNode);
                    neighbor.parent = currentNode;

                    if (!openSet.Contains(neighbor))
                    {
                        openSet.Add(neighbor);
                    }
                }
            }
        }

        return null; // 未找到路径
    }

    // 回溯构建路径
    private static List<Node> RetracePath(Node startNode, Node endNode)
    {
        List<Node> path = new List<Node>();
        Node currentNode = endNode;

        while (currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        path.Reverse();
        return path;
    }

    // 计算两个节点之间的曼哈顿距离
    private static int GetDistance(Node nodeA, Node nodeB)
    {
        int dstX = Math.Abs(nodeA.x - nodeB.x);
        int dstY = Math.Abs(nodeA.y - nodeB.y);
        return dstX + dstY;
    }

    // 示例用法
    public static void Main(string[] args)
    {
        // 创建测试网格
        Node[,] grid = new Node[5, 5];
        for (int x = 0; x < 5; x++)
        {
            for (int y = 0; y < 5; y++)
            {
                grid[x, y] = new Node(x, y, true);
            }
        }

        // 设置起点和终点
        Node startNode = grid[0, 0];
        Node targetNode = grid[4, 4];

        // 添加邻居关系（四方向）
        for (int x = 0; x < 5; x++)
        {
            for (int y = 0; y < 5; y++)
            {
                if (x > 0) grid[x, y].neighbors.Add(grid[x - 1, y]);
                if (x < 4) grid[x, y].neighbors.Add(grid[x + 1, y]);
                if (y > 0) grid[x, y].neighbors.Add(grid[x, y - 1]);
                if (y < 4) grid[x, y].neighbors.Add(grid[x, y + 1]);
            }
        }

        // 执行A*算法
        List<Node> path = FindPath(startNode, targetNode);

        if (path != null)
        {
            Console.WriteLine("找到路径:");
            foreach (Node node in path)
            {
                Console.WriteLine($"({node.x}, {node.y})");
            }
        }
        else
        {
            Console.WriteLine("未找到路径");
        }
    }
}